﻿#include "FITKPSModelSimpleShape.h"

#include "FITK_Kernel/FITKCore/FITKVec3D.h"
#include "FITK_Kernel/FITKCore/FITKVec3DAlg.h"

#include <QDebug>

#include <parasolid_kernel.h>

namespace PS
{
    void getAxis(double* location, double* point)
    {
        Core::FITKVec3D  axis3(location);
        Core::FITKVec3D  axis1(1, 0, 0);
        double ang = Core::AngleBetweenVec(axis3, axis1);
        //轴向与x 平行
        if (fabs(ang) < 1e-6 || fabs(ang - FITK_PI) < 1e-6)
            axis1.setCoor(0, 1, 0);
        //计算轴线，使其与轴线两两垂直
        Core::FITKVec3D axis2 = Core::CrossProduct(axis3, axis1);
        axis1 = Core::CrossProduct(axis2, axis3);
        //复制值
        for (int i = 0; i < 3; ++i)
            point[i] = axis1[i];
        for (int i = 0; i < 3; ++i)
            point[i + 3] = axis2[i];
    }

    FITKPSModelBox::FITKPSModelBox():
        PSShapeAgent(this)
    {
        _shapeAgent = _paraSolidShapeAgent;
    }

    bool FITKPSModelBox::update()
    {
        double x = _lengthParam[0] - _point1[0];
        double y = _lengthParam[1] - _point1[1];
        double z = _lengthParam[2] - _point1[2];

        double locat[3] = { 0,0,1 };
        double axis[6] = { 0,0,0,0,0,0 };
        getAxis(locat, axis);
        PK_AXIS2_sf_t info;
        info.location.coord[0] = _point1[0];
        info.location.coord[1] = _point1[1];
        info.location.coord[2] = _point1[2];
        info.axis.coord[0] = axis[0];
        info.axis.coord[1] = axis[1];
        info.axis.coord[2] = axis[2];
        info.ref_direction.coord[0] = axis[3];
        info.ref_direction.coord[1] = axis[4];
        info.ref_direction.coord[2] = axis[5];

        PK_BODY_t shape;
        PK_ERROR_t result = PK_BODY_create_solid_block(x, y, z, &info, &shape);
        if (result == PK_ERROR_no_errors) {
            _paraSolidShapeAgent->updateShape(shape);
            return true;
        }

        return false;
    }

    FITKPSModelCylinder::FITKPSModelCylinder():
        PSShapeAgent(this)
    {
        _shapeAgent = _paraSolidShapeAgent;
    }

    bool FITKPSModelCylinder::update()
    {
        double radius = _radius;
        double length = _length;

        double axis[6] = { 0,0,0,0,0,0 };
        getAxis(_direction, axis);

        PK_AXIS2_sf_t info;
        info.location.coord[0] = _location[0];
        info.location.coord[1] = _location[0];
        info.location.coord[2] = _location[0];
        info.axis.coord[0] = axis[0];
        info.axis.coord[1] = axis[1];
        info.axis.coord[2] = axis[2];
        info.ref_direction.coord[0] = axis[3];
        info.ref_direction.coord[1] = axis[4];
        info.ref_direction.coord[2] = axis[5];

        PK_BODY_t shape;
        PK_ERROR_t result = PK_BODY_create_solid_cyl(radius, length, &info, &shape);
        if (result == PK_ERROR_no_errors) {
            _paraSolidShapeAgent->updateShape(shape);
            return true;
        }
        return false;
    }

    FITKPSModelSphere::FITKPSModelSphere():
        PSShapeAgent(this)
    {
        _shapeAgent = _paraSolidShapeAgent;
    }

    bool FITKPSModelSphere::update()
    {
        double radius = _radius;
        PK_AXIS2_sf_t info;
        info.location.coord[0] = _location[0];
        info.location.coord[1] = _location[1];
        info.location.coord[2] = _location[2];
        info.axis.coord[0] = 0;
        info.axis.coord[1] = 0;
        info.axis.coord[2] = 1;
        info.ref_direction.coord[0] = 0;
        info.ref_direction.coord[1] = 1;
        info.ref_direction.coord[2] = 0;

        PK_BODY_t shape;
        PK_ERROR_t result = PK_BODY_create_solid_sphere(radius, &info, &shape);
        if (result == PK_ERROR_no_errors) {
            _paraSolidShapeAgent->updateShape(shape);
            return true;
        }
        return false;
    }
}

